#pragma once
#include <iostream>
#include <vector>
#include <cmath>

struct State
{
	double x;
	double y;
	double yaw;
	int dir;
	double kappa;
	State() {}
	State(double x_, double y_, double yaw_, int dir_, double kappa_)
		: x(x_), y(y_), yaw(yaw_), dir(dir_), kappa(kappa_) {}
};

void getPath(std::vector<State>& path)
{
	double straight_len = 1.5; // 直线长度1.5m
	double radius = 1; // 圆弧半径1m
	double arc_angle = M_PI / 3; // 圆弧角度60°
	double start_x = -0.52;
	double start_y = 0.26;

	// 测试路径1，直线加圆弧，直线进圆弧退，直线圆弧相切
	for (double step = start_x; step < start_x + straight_len; step += 0.01)
	{
		path.emplace_back(step, start_y, 0, 1, 0);
	}
	for (double phi = 0; phi <= arc_angle; phi += M_PI / 360) // 每隔3°一个点
	{
		double x = start_x + straight_len - radius * sin(phi);
		double y = start_y - radius * (1 - cos(phi));
		path.emplace_back(x, y, phi, -1, 1 / radius);
	}

	// 测试路径2，前进，直线加顺时针圆弧，圆弧与直线相切
	// for (double step = start_x; step < start_x + straight_len; step += 0.01)
	// {
	// 	path.emplace_back(step, start_y, 0, 1, 0);
	// }
	// for (double phi = 0; phi <= M_PI; phi += M_PI / 360)
	// {
	// 	double x = start_x + straight_len + radius * sin(phi);
	// 	double y = start_y - radius * (1 - cos(phi));
	// 	path.emplace_back(x, y, -phi, 1, 1 / radius);
	// }

	// 测试路径3，前进，直线加逆时针圆弧，圆弧与直线相切
	// for (double step = start_x; step < start_x + straight_len; step += 0.01)
	// {
	// 	path.emplace_back(step, start_y, 0, 1, 0);
	// }
	// for (double phi = 0; phi <= M_PI; phi += M_PI / 360)
	// {
	// 	double x = start_x + straight_len + radius * sin(phi);
	// 	double y = start_y + radius * (1 - cos(phi));
	// 	path.emplace_back(x, y, phi, 1, 1 / radius);
	// }

	// 测试路径4，后退直线段
	// for (double step = start_x; step > start_x - straight_len; step -= 0.01)
	// {
	// 	path.emplace_back(step, start_y, 0, -1, 0);
	// }

	// 测试路径6，后退顺时针单圆弧  // 圆弧最左侧出现画圈现象
	// for (double phi = 0; phi <= M_PI; phi += M_PI / 360)
	// {
	// 	double x = start_x - radius * sin(phi);
	// 	double y = start_y + radius * (1 - cos(phi));
	// 	path.emplace_back(x, y, -phi, -1, 1 / radius);
	// }

	// 测试路径7，后退逆时针单圆弧
	// for (double phi = 0; phi <= M_PI; phi += M_PI / 360)
	// {
	// 	double x = start_x - radius * sin(phi);
	// 	double y = start_y - radius * (1 - cos(phi));
	// 	path.emplace_back(x, y, phi, -1, 1 / radius);
	// }
}
